import matplotlib.pyplot as plt
import numpy as np

# 智能体的位置和目标位置
agent0_start = np.array([200, 400])
agent0_goal = np.array([2260, 400])
agent0_conflict = np.array([652, 402])

agent3_start = np.array([425, 530])
agent3_goal = np.array([2130, 325])
agent3_conflict = np.array([697, 412])

# 创建绘图
plt.figure(figsize=(10, 6))

# 绘制智能体0的起始位置、目标位置和冲突位置
plt.plot(
    [agent0_start[0], agent0_goal[0]],
    [agent0_start[1], agent0_goal[1]],
    "b--",
    label="Agent 0 Path",
)
plt.scatter(agent0_start[0], agent0_start[1], c="b", marker="o", label="Agent 0 Start")
plt.scatter(agent0_goal[0], agent0_goal[1], c="b", marker="x", label="Agent 0 Goal")
plt.scatter(
    agent0_conflict[0], agent0_conflict[1], c="b", marker="s", label="Agent 0 Conflict"
)

# 绘制智能体3的起始位置、目标位置和冲突位置
plt.plot(
    [agent3_start[0], agent3_goal[0]],
    [agent3_start[1], agent3_goal[1]],
    "r--",
    label="Agent 3 Path",
)
plt.scatter(agent3_start[0], agent3_start[1], c="r", marker="o", label="Agent 3 Start")
plt.scatter(agent3_goal[0], agent3_goal[1], c="r", marker="x", label="Agent 3 Goal")
plt.scatter(
    agent3_conflict[0], agent3_conflict[1], c="r", marker="s", label="Agent 3 Conflict"
)

# 添加图例和标签
plt.legend()
plt.xlabel("X Position")
plt.ylabel("Y Position")
plt.title("Conflict between Agent 0 and Agent 3")
plt.grid(True)

# 显示绘图
plt.show()
